//////////////////// Movement Functions ////////////////////
////													////	
////	Jared Bayne / Jake A O'Connor					////
////													////
////		Note:										////
////			- Use same format for each function.	////
////			- "North" is Red/Green edge. (Lab north)////
////													////
////													////
////////////////////////////////////////////////////////////

#define	CONFIRM_EDGE_REG 6 	//# of edge detections to confirm we're at the edge
#define	CONFIRM_EDGE_FRONT 12 	//# of edge detections to confirm we're at the edge

#define	CORRECT_OFFSET_RIGHT	100	//Amount of speed to subtract when following edge and not at edge
#define	CORRECT_OFFSET_LEFT		75	//Amount of speed to subtract when following edge and not at edge

#define	START_ARC_OFFSET_RIGHT	120	//Amount of speed to subtract to arc right
#define START_ARC_OFFSET_LEFT	100	//Amount of speed to subtract to ard left

#define	EMERGENCY_CORRECT_NEEDED_EDGE_FOLLOW	35	//amount of wheel encoder ticks until we emergency correct on edge follow
#define	TOO_MANY_CORRECTS_ON_EDGE_FOLLOW		2	//if the encoders don't go this many ticks in between arc starts on edge following, we need to emergency correct by turning away from edge
#define	TOO_MANY_CORRECTS_ON_EDGE_FOLLOW_CORRECT_DELAY	5	//amount to turn away from edge on "TOO_MANY_CORRECTS_ON_EDGE_FOLLOW"


//"Public" functions
void do_circles_around_track(int speed, int which_way);

void begin_to_one(int speed);		//works - need to incorporate line sensors
void begin_to_two(int speed);		//works - need to incorporate line sensors
void begin_to_three(int speed);		//works - need to incorporate line sensors
void begin_to_four(int speed);		//works - need to incorporate line sensors

void red_to_one(int speed);			//works nonrobustly- need to incorporate line sensors
void red_to_two(int speed);			//works nonrobustly- need to incorporate line sensors
void red_to_three(int speed);		//works nonrobustly- need to incorporate line sensors
void red_to_four(int speed);		//works nonrobustly- need to incorporate line sensors

void blue_to_one(int speed);		//TODO: test
void blue_to_two(int speed);		//TODO: test
void blue_to_three(int speed);		//TODO: test
void blue_to_four(int speed);		//TODO: test

void green_to_one(int speed);		//TODO: test
void green_to_two(int speed);		//TODO: test
void green_to_three(int speed);		//TODO: test
void green_to_four(int speed);		//TODO: test

void yellow_to_one(int speed);		//TODO: test
void yellow_to_two(int speed);		//TODO: test
void yellow_to_three(int speed);	//TODO: test
void yellow_to_four(int speed);		//TODO: test

void one_to_red(int speed);			//TODO: wait on line sensors
void one_to_blue(int speed);		//TODO: wait on line sensors
void one_to_green(int speed);		//TODO: wait on line sensors
void one_to_yellow(int speed);		//TODO: wait on line sensors

void two_to_red(int speed);			//TODO: wait on line sensors
void two_to_blue(int speed);		//TODO: wait on line sensors
void two_to_green(int speed);		//TODO: wait on line sensors
void two_to_yellow(int speed);		//TODO: wait on line sensors

void three_to_red(int speed);		//TODO: wait on line sensors
void three_to_blue(int speed);		//TODO: wait on line sensors
void three_to_green(int speed);		//TODO: wait on line sensors
void three_to_yellow(int speed);	//TODO: wait on line sensors

void four_to_red(int speed);		//TODO: wait on line sensors
void four_to_blue(int speed);		//TODO: wait on line sensors
void four_to_green(int speed);		//TODO: wait on line sensors
void four_to_yellow(int speed);		//TODO: wait on line sensors



//"Private" functions
// These are the main functions for edge-related movement
void go_forward_to_edge(int speed);		//works
void go_right_to_edge(int speed, int sensor);		//works
void go_left_to_edge_ARC(int speed, int sensor);	//works
void go_right_to_edge_ARC(int speed,int sensor);
void go_left_to_edge(int speed, int sensor);		//works
void go_right_to_track(int speed, int sensor);		//works
void go_left_to_track(int speed, int sensor);		//works
void go_right_to_track_box_version(int speed,int sensor);
void go_right_to_edge_ARC_box_version(int speed, int sensor);
void follow_edge_on_right(int speed);	//works - update with emergency function (i.e. if we don't see edge for X wheel ecoder ticks we are way off and need to do drastic fix)
void follow_edge_on_left(int speed);	//works - update with emergency function (i.e. if we don't see edge for X wheel ecoder ticks we are way off and need to do drastic fix)

void goto_box(int speed);
int are_we_at_the_box(int mode);
void doStuff(int speed);
void here_to_there(int here, int there, int direction);

void doStuff(int speed)
{
	//This function should be run once calibration is done
	//	and menu function has been called to input sequence.
	//Relies on the global array "roomOrderToGoTo[]" for function.
	//Should move from room to room in sequence specified in the global
	//	array.  Go to room -> line up -> lower arm -> read color -> go to box -> line up -> raise arm (x4)
	//Uses a huge series of switches to determine required path from room to room.
	////////////////////////////////////////////////
	//TODO: NEEDS FUNCTIONS ADDED, SEE BELOW!!!	  //
	//		"//==>" == Lower robot arm function	  //
	//		"//==^" == Raise robot arm function	  //
	////////////////////////////////////////////////
	
	int here = -1, tmp=0;	//Bot's current position
	
	for(int i=0; i<4; i++)	//For each path in the sequence
	{
	here_to_there(here, roomOrderToGoTo[i], 1);	//Go from here to the next corner
	here = roomOrderToGoTo[i];	//Now we're at this corner
	//lower robot arm
//	tmp = read_color_sensor();	//Take a color reading on the crate
	here_to_there(here, tmp, 0);	//Go from here the correct color box
	here = tmp;	//Now we're at this
	//raise robot arm
	}
	
}



int are_we_at_the_box(int mode)
{

//I know defines shouldn't be here, but you try programming
//a bot the night before competition and have it hardly working!

#define	all		1
#define	no_line	2

	int there_yet = NO;

	if (mode == all)
	{	
		if (how_many_see_black()>0)
		{
			there_yet = YES;
			writeln("saw black");
		}
		//if (color_sensor_see_box()==YES)
		//	there_yet = YES;
		
		if (edge_sensor_see_box()==YES)
		{
			there_yet = YES;
			writeln("Saw Box");
		}
		return there_yet;
	}
	else if (mode==no_line)
	{
		//if (color_sensor_see_box()==YES)
		//	there_yet = YES;
		
		if (edge_sensor_see_box()==YES)
		{
			there_yet = YES;
			writeln("Saw Box");
		}
		return there_yet;
	
	}
	
	writeln("should never reach here");
	return there_yet;
}


void goto_box(int speed)
{
	//we are assuming that we are starting from the end position
	//of the edge following routine.  So, we just got "to the room"
	//in a room function and ended by "seeing edge" as opposed to 
	//"seeing black line" of box.

	writeln("Finding Box");
	go_right_to_edge_ARC_box_version(speed, edge_drive_front);
	reset_encoders();
	while(!are_we_at_the_box(all) && (average_all_encoders()<=2))
		goright(speed);
	
	go_right_to_track_box_version(speed, edge_drive_front);

	while(!are_we_at_the_box(all) && (average_all_encoders()<=5))
		goleft(speed);

	reset_encoders();
	while ( (average_all_encoders()<3) && (are_we_at_the_box(no_line)==NO))
	{
		goforward(speed-20);
	
	}
	while(!are_we_at_the_box(all))
		goforward_perfectlystraight_continuous(speed-20);
	
	gobackward(speed);
	encoder_delay(2);

}

/////////////////////////////////////////////////////////////
///////////// START POSITION TO CORNERS /////////////////////
void begin_to_one(int speed)
{	
/*	goright(speed);	//Turn right a little bit so we're facing north-west
	encoder_delay(3);

	go(Forward,speed,Forward,speed-START_ARC_OFFSET_RIGHT);	//Go forward a little bit to get us away from the edge
	encoder_delay(5);
	
	
	go(Forward,speed,Forward,speed-START_ARC_OFFSET_RIGHT);	//sharp arc right for a while
	encoder_delay(55);	//this delay is fairly arbitrary

	go_forward_to_edge(speed);
*/
	while(!how_many_see_black()==0)
		goleft(REGULAR_SPEED);
	stop();
	while(1);
	reset_encoders();
	while(how_many_see_black()>3)
		follow_edge_on_right(speed);
		

}


void begin_to_two(int speed)
{
	go_right_to_edge(speed, edge_pass_front);
	reset_encoders();
	while(how_many_see_black()==0)
		follow_edge_on_right(speed);

}


void begin_to_three(int speed)
{
	goleft(speed);	//Turn left a little bit so we're facing south-west
	encoder_delay(2);

	go(Forward,speed-START_ARC_OFFSET_LEFT,Forward,speed);	//Go forward a little bit to get us away from the edge
	encoder_delay(5);
	
	
	go(Forward,speed-START_ARC_OFFSET_LEFT,Forward,speed);	//sharp arc left for a while
	encoder_delay(55);	//this delay is fairly arbitrary
	
	go_forward_to_edge(speed);
	
	
	while(how_many_see_black()>0)
		goright(REGULAR_SPEED);
	reset_encoders();
	while(how_many_see_black()==0)
		follow_edge_on_left(speed);
}

void begin_to_four(int speed)
{
	go_left_to_edge(speed, edge_drive_front);
	reset_encoders();
	while(how_many_see_black()==0)
		follow_edge_on_right(speed);
}


/////////////////////////////////////////////////////////////
//////////////////// RED BOX TO CORNERS /////////////////////

void red_to_one(int speed)
{
	writeln("Red to One");
	goleft(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);
	
	goleft(speed);
	encoder_delay(2);
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge

	
	//reset_encoders();
	//do{
		//writeln("Follow Edge");
		//while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
		while(how_many_see_black()==0)
			follow_edge_on_right(speed);
		//writeln("Conf End");
		
		gobackward(speed);
		encoder_delay(5);
		stop();
	//}while(!super_confirm_see_floor(5000));
	
	writeln("At One");
}

void red_to_two(int speed)
{
	goleft(speed);
	encoder_delay(26);

	

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge

	go_right_to_edge_ARC(speed, edge_drive_front);
	go_right_to_track(speed, edge_drive_front);
	//Now pretty much parallel to north edge
	//writeln("Follow Edge");
	reset_encoders();
	do{
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_left(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

	//writeln("At room 2");
}

void red_to_three(int speed)
{
	goright(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge
	
	go_left_to_edge(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	


	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void red_to_four(int speed)
{
	//go away from red box
	gobackward(speed);
	encoder_delay(15);

	//turn right so we can hit south edge
	goright(speed);
	encoder_delay(7);

	//make slight arc until we hit south edge
	//arc will make impact soft
	go_forward_to_edge(speed);

	//No facing perpinduclar to north edge
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	

	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}


/////////////////////////////////////////////////////////////
//////////////////// BLUE BOX TO CORNERS ////////////////////

void blue_to_one(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/
	goleft(speed);
	encoder_delay(26);

	

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge

	go_right_to_edge_ARC(speed, edge_drive_front);
	go_right_to_track(speed, edge_drive_front);
	//Now pretty much parallel to north edge
	//writeln("Follow Edge");
	reset_encoders();
	do{
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_left(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

	//writeln("At room 1");
}

void blue_to_two(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoders
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
	{
		go(Backward,speed,Backward,speed);
	}
	while(!double_check_edge(edge_drive_front, CONFIRM_EDGE_REG) && !double_check_edge(edge_front, CONFIRM_EDGE_REG))
	{
		//arc to the left until we see edge
		go(Forward,speed-START_ARC_OFFSET_LEFT,Forward,speed);
	}
	goright(speed);
	_delay_ms(200);	//Update with wheel encoder
	while(1)
		follow_edge_on_left(speed);
*/
	//go away from red box
	gobackward(speed);
	encoder_delay(15);

	//turn right so we can hit south edge
	goright(speed);
	encoder_delay(7);

	//make slight arc until we hit south edge
	//arc will make impact soft
	go_forward_to_edge(speed);

	//No facing perpinduclar to north edge
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	

	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void blue_to_three(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/

	goleft(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to west edge

	
	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));
}

void blue_to_four(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_right_to_edge(speed);
	while(1)
		follow_edge_on_right(speed);
*/
	goright(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);

	//Now facing perpinduclar to south edge
	
	go_left_to_edge(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to south edge
	


	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}


/////////////////////////////////////////////////////////////
/////////////////// GREEN BOX TO CORNERS ////////////////////

void green_to_one(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_right_to_edge(speed);
	while(1)
		follow_edge_on_right(speed);
*/
	goright(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge
	
	go_left_to_edge(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	


	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void green_to_two(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/

	goleft(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to east edge

	
	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));
}

void green_to_three(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoders
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
	{
		go(Backward,speed,Backward,speed);
	}
	while(!double_check_edge(edge_drive_front, CONFIRM_EDGE_REG) && !double_check_edge(edge_front, CONFIRM_EDGE_REG))
	{
		//arc to the left until we see edge
		go(Forward,speed-START_ARC_OFFSET_LEFT,Forward,speed);
	}
	goright(speed);
	_delay_ms(200);	//Update with wheel encoder
	while(1)
		follow_edge_on_left(speed);
*/
	//go away from red box
	gobackward(speed);
	encoder_delay(15);

	//turn right so we can hit south edge
	goright(speed);
	encoder_delay(7);

	//make slight arc until we hit south edge
	//arc will make impact soft
	go_forward_to_edge(speed);

	//No facing perpinduclar to north edge
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	

	reset_encoders();
	do{
		//writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		//writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void green_to_four(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/
	goleft(speed);
	encoder_delay(26);

	

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge

	go_right_to_edge_ARC(speed, edge_drive_front);
	go_right_to_track(speed, edge_drive_front);
	//Now pretty much parallel to north edge
	writeln("Follow Edge");
	reset_encoders();
	do{
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_left(speed);
		writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

	writeln("At room 4");
}


/////////////////////////////////////////////////////////////
////////////////// YELLOW BOX TO CORNERS ////////////////////

void yellow_to_one(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoders
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
	{
		go(Backward,speed,Backward,speed);
	}
	while(!double_check_edge(edge_drive_front, CONFIRM_EDGE_REG) && !double_check_edge(edge_front, CONFIRM_EDGE_REG))
	{
		//arc to the left until we see edge
		go(Forward,speed-START_ARC_OFFSET_LEFT,Forward,speed);
	}
	goright(speed);
	_delay_ms(200);	//Update with wheel encoder
	while(1)
		follow_edge_on_left(speed);
*/
	//go away from red box
	gobackward(speed);
	encoder_delay(15);

	//turn right so we can hit south edge
	goright(speed);
	encoder_delay(7);

	//make slight arc until we hit south edge
	//arc will make impact soft
	go_forward_to_edge(speed);

	//No facing perpinduclar to north edge
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to north edge
	

	reset_encoders();
	do{
		writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void yellow_to_two(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_right_to_edge(speed);
	while(1)
		follow_edge_on_right(speed);
*/
	goright(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);

	//Now facing perpinduclar to east edge
	
	go_left_to_edge(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to east edge
	


	reset_encoders();
	do{
		writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

}

void yellow_to_three(int speed)
{
/*
	goright(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/
	goleft(speed);
	encoder_delay(26);

	

	go_forward_to_edge(speed);

	//Now facing perpinduclar to north edge

	go_right_to_edge_ARC(speed, edge_drive_front);
	go_right_to_track(speed, edge_drive_front);
	//Now pretty much parallel to north edge
	writeln("Follow Edge");
	reset_encoders();
	do{
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_left(speed);
		writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));

	writeln("At room 3");
}

void yellow_to_four(int speed)
{
/*
	goleft(speed);
	_delay_ms(250);	//Update with wheel encoder
	while(!double_check_edge(edge_back, CONFIRM_EDGE_REG))
		go(Backward,speed,Backward,speed);
	go_left_to_edge(speed);
	while(1)
		follow_edge_on_left(speed);
*/
	goleft(speed);
	encoder_delay(26);

	go_forward_to_edge(speed);
	
	go_left_to_edge_ARC(speed, edge_pass_front);
	go_left_to_track(speed, edge_pass_front);
	//Now pretty much parallel to south edge

	
	reset_encoders();
	do{
		writeln("Follow Edge");
		while( !double_check_edge(edge_front_right, CONFIRM_EDGE_FRONT) && !double_check_edge(edge_front_left, CONFIRM_EDGE_FRONT))	//update stopping condition with line sensors
			follow_edge_on_right(speed);
		writeln("Conf End");
		stop();
	}while(!super_confirm_see_floor(5000));
}


/////////////////////////////////////////////////////////////
/////////////////// CORNER ONE TO CENTER ////////////////////

void one_to_red(int speed)
{
	reset_encoders();

	gobackward(speed);
	encoder_delay(15);
	
	goleft(speed);
	encoder_delay(10);
	
	while(!does_sense_see_edge(edge_pass_front))
	{
		goforward(speed);
		goleft(speed);
	}
	
	go_left_to_track(speed, edge_pass_front);
	goleft(speed);
	encoder_delay(5);
	
	while(how_many_see_black() < 3)
		goforward(speed);
	
}		


void one_to_blue(int speed)
{
}

void one_to_green(int speed)
{
}

void one_to_yellow(int speed)
{
}


/////////////////////////////////////////////////////////////
/////////////////// CORNER TWO TO CENTER ////////////////////


void two_to_red(int speed)
{
}

void two_to_blue(int speed)
{
}

void two_to_green(int speed)
{
}

void two_to_yellow(int speed)
{
}


/////////////////////////////////////////////////////////////
///////////////// CORNER THREE TO CENTER ////////////////////

void three_to_red(int speed)
{
}

void three_to_blue(int speed)
{
}

void three_to_green(int speed)
{
}

void three_to_yellow(int speed)
{
}


/////////////////////////////////////////////////////////////
////////////////// CORNER FOUR TO CENTER ////////////////////

void four_to_red(int speed)
{
}

void four_to_blue(int speed)
{
}

void four_to_green(int speed)
{
}

void four_to_yellow(int speed)
{
}



///////////////////////////////////////////////////////////////////////////////
////////////////// OTHER FUNCTIONS NEEDED (OR NOT) ////////////////////////////
///////////////////////////////////////////////////////////////////////////////



void follow_edge_on_left(int speed)	////Done, tested ////
{
	/*
		!!MUST CALL reset_encoders() right before you call this function!!
	
		DESCRIPTION:
		Attempts to follow the edge of the track when
		the edge is on the left side of the robot.
		
		What's the deal with this emergency correct stuff?
			-	If the robot doesn't see the edge after
				a certain number of wheel encoder ticks 
				then we are most likely way off the edge
				and need to do a drastic correction.
			
			-	Right now, this drastic correction consists
				of just turning in towards the edge.
				
			-	During the drastic correction, we watch out
				edge sensors to make sure we aren't driving
				off the board.
		
		ALGORITHM:
			The general algorithm used in this edge following
			routine is as follows:
				
				- arc towards the edge
				- if see edge
					-turn away from edge until we don't see edge
				- repeat
				
			We are making semicircles all around the board.
			The goal is to make these semicircles so small
			that the human eye can't make out that we are even
			doing semicircles.  We want it to look like the robot
			is just following along the edge perfectly parallel
			to the edge.
		
		SENSORS USED: 
			edge_drive_front
		
		EXAMPLE USAGE:		
			
			while(Enc1<100)	//follow edge for 100 wheel encoder ticks
				follow_edge_on_left(speed);				
	*/
	int checker=0;
	
	//first check if we are way away from edge and need an emergency correction
	if (average_all_encoders()>EMERGENCY_CORRECT_NEEDED_EDGE_FOLLOW)
	{
		//need to correct drastically because we are way away from edge
	
		//in no man's land right now (most likely)
		reset_encoders();
		while ( average_all_encoders()<5 && double_check_edge_all(CONFIRM_EDGE_REG, CONFIRM_EDGE_FRONT))
			goleft(speed);
			
		//pointed towards track (should be able to correct better now)
		reset_encoders();	//so we can watch for a future emergency correct	
	}
	if (does_sense_see_edge(edge_drive_front))	//got one reading that says edge
	{
		checker=0;
		for (int i=0;i<CONFIRM_EDGE_REG-1;i++)	//see if we can get CONFRIM_EDGE readings in a row
		{
			if (!does_sense_see_edge(edge_drive_front))
				checker = 1;		
		}
		
		if (checker==0)		//we saw edge CONFIRM_EDGE_REG times	
		{
			go_right_to_track(speed, edge_drive_front);				
			reset_encoders();	//reset encoders to watch for future emergency conditions
		}
	}
	else
		go(Forward,speed-CORRECT_OFFSET_LEFT,Forward,speed);	//arc towards edge so that we hit it again
	
	



}

void follow_edge_on_right(int speed) ////Done, tested ////
{
	/*
		!!MUST CALL reset_encoders() right before you call this function!!
	
		DESCRIPTION:
		Attempts to follow the edge of the track when
		the edge is on the right side of the robot.
		
		What's the deal with this emergency correct stuff?
			-	If the robot doesn't see the edge after
				a certain number of wheel encoder ticks 
				then we are most likely way off the edge
				and need to do a drastic correction.
			
			-	Right now, this drastic correction consists
				of just turning in towards the edge.
				
			-	During the drastic correction, we watch out
				edge sensors to make sure we aren't driving
				off the board.
		
		ALGORITHM:
			The general algorithm used in this edge following
			routine is as follows:
				
				- arc towards the edge
				- if see edge
					-turn away from edge until we don't see edge
				- repeat
				
			We are making semicircles all around the board.
			The goal is to make these semicircles so small
			that the human eye can't make out that we are even
			doing semicircles.  We want it to look like the robot
			is just following along the edge perfectly parallel
			to the edge.
		
		SENSORS USED: 
			edge_pass_front
		
		EXAMPLE USAGE:		
			
			while(Enc1<100)	//follow edge for 100 wheel encoder ticks
				follow_edge_on_right(speed);				
	*/
	
	
	int checker=0;
	
	//first check if we are way away from edge and need an emergency correction
	if (average_all_encoders() > EMERGENCY_CORRECT_NEEDED_EDGE_FOLLOW)
	{
		//need to correct drastically because we are way away from edge
	
		//in no man's land right now (most likely)
		reset_encoders();
		while ( average_all_encoders() < 5 && double_check_edge_all(CONFIRM_EDGE_REG, CONFIRM_EDGE_FRONT))
			goright(speed);
			
		//pointed towards track (should be able to correct better now)
		reset_encoders();	//so we can watch for a future emergency correct	
	}
	if (does_sense_see_edge(edge_pass_front))	//got one reading that says edge
	{
		checker=0;
		for (int i=0; i < CONFIRM_EDGE_REG-1 ;i++)	//see if we can get CONFRIM_EDGE readings in a row
		{
			if (!does_sense_see_edge(edge_pass_front))
				checker = 1;		
		}
		
		if (checker==0)		//we saw edge CONFIRM_EDGE_REG times	
		{
			go_left_to_track(speed,edge_pass_front);				
			reset_encoders();	//reset encoders to watch for future emergency conditions
		}
	}
	else
		go(Forward,speed,Forward,speed-CORRECT_OFFSET_RIGHT);	//arc towards edge so that we hit it again
	
	
	
}

//Facing nothing specifically, go forward until we hit an edge
void go_forward_to_edge(int speed)	//// Done, tested ////
{	
	/*
		DESCRIPTION:
		Goes forward until any sensor sees edge
		
		SENSORS USED: 
			all
		
		EXAMPLE USAGE:		
			
			go_forward_to_edge(100);
			
		
	*/
/*
	do
	{
		goforward(speed);
		encoder_delay(10);
		while (double_check_edge_all(CONFIRM_EDGE_REG, CONFIRM_EDGE_FRONT)==YES)
			goforward(speed);
		stop();
		gobackward(speed);	//quick stop
		encoder_delay(5);
		stop();
		_delay_ms(100);
		//writeln("Confirming Edge");
	}while(!super_confirm_see_floor(5000));

*/

	goforward(speed);
	encoder_delay(4);

		while(how_many_see_black()==0)
			goforward(speed);
		
		gobackward(speed);
		encoder_delay(5);
		while (how_many_see_black()==0)
			goforward(speed-100);
		



}

//Facing the edge, turn right until we're angled to the edge (arc movement)
void go_right_to_edge_ARC(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes right until sensor sees floor
		
		This function will err on the side of stopping early.
		We would rather stop early than run off the track.
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_right_to_edge(100, edge_pass_front);
			
		
	*/
	
	//reset all thresholds
	
	#define subtract 3
	
	int temp1 = edge_front_left_threshold;
	int temp2 = edge_front_right_threshold;
	int temp3 = edge_drive_back_threshold;
	int temp4 = edge_drive_front_threshold;
	int temp5 = edge_pass_front_threshold;
	int temp6 = edge_pass_back_threshold;
	int temp7 = edge_back_threshold;
	
	edge_front_left_threshold-=subtract;
	edge_front_right_threshold-=subtract;
	edge_drive_back_threshold-=subtract;
	edge_drive_front_threshold-=subtract;
	edge_pass_front_threshold-=subtract;
	edge_pass_back_threshold-=subtract;
	edge_back_threshold-=subtract;
	
		
	goright(speed);	//Go right a little bit to get us ready
	encoder_delay(2);
	
	
	while (!double_check_edge(sensor, CONFIRM_EDGE_REG))	//Go right until our pass sensor hits the edge
		go(Forward, speed, Forward, 0);
		
	edge_front_left_threshold=temp1;
	edge_front_right_threshold=temp2;
	edge_drive_back_threshold=temp3;
	edge_drive_front_threshold=temp4;
	edge_pass_front_threshold=temp5;
	edge_pass_back_threshold=temp6;
	edge_back_threshold=temp7;
		
}

void go_right_to_edge_ARC_box_version(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes right until sensor sees floor
		
		This function will err on the side of stopping early.
		We would rather stop early than run off the track.
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_right_to_edge(100, edge_pass_front);
			
		
	*/
	
	//reset all thresholds
	
	#define subtract 3
	
	int temp1 = edge_front_left_threshold;
	int temp2 = edge_front_right_threshold;
	int temp3 = edge_drive_back_threshold;
	int temp4 = edge_drive_front_threshold;
	int temp5 = edge_pass_front_threshold;
	int temp6 = edge_pass_back_threshold;
	int temp7 = edge_back_threshold;
	
	edge_front_left_threshold-=subtract;
	edge_front_right_threshold-=subtract;
	edge_drive_back_threshold-=subtract;
	edge_drive_front_threshold-=subtract;
	edge_pass_front_threshold-=subtract;
	edge_pass_back_threshold-=subtract;
	edge_back_threshold-=subtract;
	
		
	goright(speed);	//Go right a little bit to get us ready
	encoder_delay(2);
	
	
	while (!double_check_edge(sensor, CONFIRM_EDGE_REG) && !are_we_at_the_box(all))	//Go right until our pass sensor hits the edge
		go(Forward, speed, Forward, 0);
		
	edge_front_left_threshold=temp1;
	edge_front_right_threshold=temp2;
	edge_drive_back_threshold=temp3;
	edge_drive_front_threshold=temp4;
	edge_pass_front_threshold=temp5;
	edge_pass_back_threshold=temp6;
	edge_back_threshold=temp7;
		
}

//Facing the edge, turn right until we're angled to the edge
void go_right_to_edge(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes right until sensor sees floor
		
		This function will err on the side of stopping early.
		We would rather stop early than run off the track.
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_right_to_edge(100, edge_pass_front);
			
		
	*/
	goright(speed);	//Go right a little bit to get us ready
	encoder_delay(2);
	
	
	while (!double_check_edge(sensor, CONFIRM_EDGE_REG))	//Go right until our pass sensor hits the edge
		goright(speed);
		//go(Forward(
}

void go_left_to_edge_ARC(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes left until sensor sees floor
		
		This function will err on the side of stopping early.
		We would rather stop early than run off the track.
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_left_to_edge(100, edge_pass_front);
			
		
	*/
	int temp1 = edge_front_left_threshold;
	int temp2 = edge_front_right_threshold;
	int temp3 = edge_drive_back_threshold;
	int temp4 = edge_drive_front_threshold;
	int temp5 = edge_pass_front_threshold;
	int temp6 = edge_pass_back_threshold;
	int temp7 = edge_back_threshold;
	
	edge_front_left_threshold-=subtract;
	edge_front_right_threshold-=subtract;
	edge_drive_back_threshold-=subtract;
	edge_drive_front_threshold-=subtract;
	edge_pass_front_threshold-=subtract;
	edge_pass_back_threshold-=subtract;
	edge_back_threshold-=subtract;
	
	
	goleft(speed);	//Go right a little bit to get us ready
	encoder_delay(2);
	
	
	while (!double_check_edge(sensor, CONFIRM_EDGE_REG))	//Go right until our pass sensor hits the edge
		go(Forward, 0, Forward, speed);
		
	edge_front_left_threshold=temp1;
	edge_front_right_threshold=temp2;
	edge_drive_back_threshold=temp3;
	edge_drive_front_threshold=temp4;
	edge_pass_front_threshold=temp5;
	edge_pass_back_threshold=temp6;
	edge_back_threshold=temp7;

}

//Facing the edge, turn left until we're angled to the edge
void go_left_to_edge(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes left until sensor sees floor
		
		This function will err on the side of stopping early.
		We would rather stop early than run off the track.
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_left_to_edge(100, edge_drive_front);
			
		
	*/
	goleft(speed);	//Go left a little bit to get us ready
	encoder_delay(2);
	
	while (!double_check_edge(sensor, CONFIRM_EDGE_REG))	//Go left until our drive sensor hits the edge
		goleft(speed);

}

void go_right_to_track_box_version(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes right until sensor sees track
		
		Will usually use this function while watching the edge_drive_front sensor
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_right_to_track(100, edge_drive_front);
			
		
	*/
	goright(speed);
	encoder_delay(0);
	while (!double_check_track(sensor, CONFIRM_EDGE_REG) && !are_we_at_the_box(all))	//Go right until our drive sensor hits the track again
		goright(speed);
}

//edge_drrive_front sees floor, turn until it sees track
void go_right_to_track(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes right until sensor sees track
		
		Will usually use this function while watching the edge_drive_front sensor
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_right_to_track(100, edge_drive_front);
			
		
	*/
	goright(speed);
	encoder_delay(0);
	while (!double_check_track(sensor, CONFIRM_EDGE_REG))	//Go right until our drive sensor hits the track again
		goright(speed);
}


//Sensor sees floor, turn until it sees track
void go_left_to_track(int speed,int sensor)	//// Done, tested ////
{
	/*
		DESCRIPTION:
		Goes left until sensor sees track
		
		Will usually use this function while watching the edge_pass_front sensor
		
		SENSORS USED: 
			sensor (whatever we pass to function)
		
		EXAMPLE USAGE:		
			
			go_left_to_track(100, edge_pass_front);
			
		
	*/
	goleft(speed);
	encoder_delay(0);
	while (!double_check_track(sensor, CONFIRM_EDGE_REG))	//Go left until our pass sensor hits the track again
		goleft(speed);

}


//Run around like a maniac
void do_circles_around_track(int speed, int which_way)
{

	/*
		DESCRIPTION:
		All this function does is continually test the edge
		following routines.  When the robot gets to a corner
		it will simply turn and continue following the next
		edge of the track.
		
		This function would never be used in the actual code.
		
		Tester function only.
		
		SENSORS USED: 
			edge_drive_front	-> left_follow
			edge_pass_front	 	-> right follow
		
		EXAMPLE USAGE:		
			
			while(1)
				do_circles_around_track(right);
			
		
	*/
/*
	switch (which_way)
	{
		case right:
			
			while(!double_check_edge(edge_front, CONFIRM_EDGE_REG))
				correction = follow_edge_on_right(speed, correction);
			goleft(speed);
			//encoder_delay(4);
			break;
		
		case left:
			while(!double_check_edge(edge_front, CONFIRM_EDGE_REG))
				follow_edge_on_left(speed);
			gobackward(speed);
			//encoder_delay(6);
			go_right_to_edge(speed, edge_drive_front);
			go_right_to_track(speed, edge_drive_front);
			break;
			
		default:
		
			writeln("ERROR");
			break;
			
	}

*/


}


void here_to_there(int here, int there, int direction)
{	//Decodes given "here" and "there" and runs the function accordingly
	//Direction: '0' = From Corners to Center, '1' = From Center/Start to Corners

	if(direction == 0)
	{
		switch(here)
		{
		case 1:		//Outside Corner 1
			switch(there)
			{
			case red: one_to_red(REGULAR_SPEED); break;
			case green:  one_to_green(REGULAR_SPEED); break;
			case yellow:  one_to_yellow(REGULAR_SPEED); break;
			case blue:  one_to_blue(REGULAR_SPEED); break;
			}
			break;
		
		case 2:		//Outside Corner 2
			switch(there)
			{
			case red: two_to_red(REGULAR_SPEED); break;
			case green:  two_to_green(REGULAR_SPEED); break;
			case yellow:  two_to_yellow(REGULAR_SPEED); break;
			case blue:  two_to_blue(REGULAR_SPEED); break;
			}
			break;
		
		case 3:		//Outside Corner 3
			switch(there)
			{
			case red: three_to_red(REGULAR_SPEED); break;
			case green:  three_to_green(REGULAR_SPEED); break;
			case yellow:  three_to_yellow(REGULAR_SPEED); break;
			case blue:  three_to_blue(REGULAR_SPEED); break;
			}
			break;
		
		case 4:		//Outside Corner 4
			switch(there)
			{
			case red: four_to_red(REGULAR_SPEED); break;
			case green:  four_to_green(REGULAR_SPEED); break;
			case yellow:  four_to_yellow(REGULAR_SPEED); break;
			case blue: four_to_blue(REGULAR_SPEED); break;
			}
			break;
		
		}
	}
	else
	{
		switch(here)
		{
		case -1:	//Beginning Position
			switch(there)
			{
			case red: begin_to_one(REGULAR_SPEED); break;
			case green:  begin_to_two(REGULAR_SPEED); break;
			case yellow:  begin_to_three(REGULAR_SPEED); break;
			case blue:  begin_to_four(REGULAR_SPEED); break;
			}
			break;
		
		case red:		//Outside of Red Box
			switch(there)
			{
			case 1: red_to_one(REGULAR_SPEED); break;
			case 2: red_to_two(REGULAR_SPEED); break;
			case 3: red_to_three(REGULAR_SPEED); break;
			case 4: red_to_four(REGULAR_SPEED); break;
			}
			break;
		
		case green:		//Outside of Green Box
			switch(there)
			{
			case 1: green_to_one(REGULAR_SPEED); break;
			case 2: green_to_two(REGULAR_SPEED); break;
			case 3: green_to_three(REGULAR_SPEED); break;
			case 4: green_to_four(REGULAR_SPEED); break;
			}
			break;
		
		case yellow:		//Outside of Yellow Box
			switch(there)
			{
			case 1: yellow_to_one(REGULAR_SPEED); break;
			case 2: yellow_to_two(REGULAR_SPEED); break;
			case 3: yellow_to_three(REGULAR_SPEED); break;
			case 4: yellow_to_four(REGULAR_SPEED); break;
			}
			break;
		
		case blue:		//Outside of Blue Box
			switch(there)
			{
			case 1: blue_to_one(REGULAR_SPEED); break;
			case 2: blue_to_two(REGULAR_SPEED); break;
			case 3: blue_to_three(REGULAR_SPEED); break;
			case 4: blue_to_four(REGULAR_SPEED); break;
			}
			break;
		
		}
	}
}
